perm filename ARITH.SAI[AL,HE]6 blob
sn#394537 filedate 1978-11-13 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00009 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00002 00002 ENTRY
C00007 00003 ! new_sval,new_v3ect,new_trans,new_frame
C00009 00004 ! v3dot, v3dist, v3angle, grfmul
C00012 00005 ! v3ect-valued procedures
C00019 00006 ! rotn valued procedures
C00025 00007 ! rotn extraction procedures
C00032 00008 ! trans & frame valued procedures
C00034 00009 ! v3cmp, rotcmp, & transcmp
C00036 ENDMK
C⊗;
ENTRY;
BEGIN "ARITH"
IFCR ¬DECLARATION(CREFFING) THENC DEFINE CREFFING=FALSE;ENDC
IFCR ¬CREFFING THENC
REQUIRE "ABBREV.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "MACROS.SAI[AL,HE]" SOURCE_FILE;
REQUIRE "RECAUX.HDR[AL,HE]" SOURCE_FILE;
ENDC
REDEFINE ARITHTERNAL = "INTERNAL";
REQUIRE "ARITH.HDR[AL,HE]" SOURCE_FILE;
! some initialization & "global" constants;
PROCEDURE IACNSTS;
BEGIN
RPTR(TINFO) Q;
! INITIALIZE ARITHMETIC CONSTANTS;
PROCEDURE MAKE_CONST (STRING ID; RPTR(VALU$) V);
BEGIN
EXTERNAL RCLASS DEFID(STRING NAME; RANY VAL; RPTR(DEFID) NEXT);
EXTERNAL RPTR(DEFID) SYSIDS;
RPTR(DEFID) D;
D ← NEW_RECORD(DEFID);
DEFID:NAME[D] ← ID;
DEFID:VAL[D] ← V;
DEFID:NEXT[D] ← SYSIDS;
SYSIDS ← D
END;
TRUEV←NEW_SVAL(1);
FALSEV←NEW_SVAL(0);
RADIANS←NEW_SVAL(180./π);
PI←NEW_SVAL(π);
CM←NEW_SVAL(1./2.54);
GM←NEW_SVAL(16./454.);
LBS←NEW_SVAL(16.);
NILVECT←NEW_V3ECT(0,0,0);
XHAT←NEW_V3ECT(1,0,0);
YHAT←NEW_V3ECT(0,1,0);
ZHAT←NEW_V3ECT(0,0,1);
NEGXHAT ← NEW_V3ECT(-1,0,0);
NEGYHAT ← NEW_V3ECT(0,-1,0);
NEGZHAT ← NEW_V3ECT(0,0,-1);
NILROTN←AXW_ROTN(ZHAT,0);
NILTRANS←NEW_TRANS(NILROTN,NILVECT);
NILDEPROACH ← STATION ← NEW_FRAME(NILTRANS);
ZFLIPR←VTOV_R(ZHAT,NEGZHAT);
ZFLIPT←NEW_TRANS(ZFLIPR,NILVECT);
YPARK ← NEW_FRAME( NEW_TRANS(AXW_ROTN(YHAT,180),NEW_V3ECT(40,14,9) ) );
BPARK ← NEW_FRAME( NEW_TRANS(AXW_ROTN(YHAT,180),
NEW_V3ECT(43.53125,56.855,9.95875)) );
STAN_DEPROACH ← NEW_V3ECT(0,0,3);
MAKE_CONST("TRUE",TRUEV);
MAKE_CONST("FALSE",FALSEV);
MAKE_CONST("RADIANS",RADIANS);
MAKE_CONST("PI",PI);
MAKE_CONST("π",PI);
MAKE_CONST("CM",CM);
MAKE_CONST("GM",GM);
MAKE_CONST("INCH",TRUEV);
MAKE_CONST("INCHES",TRUEV);
MAKE_CONST("SEC",TRUEV);
MAKE_CONST("SECONDS",TRUEV);
MAKE_CONST("OZ",TRUEV);
MAKE_CONST("OUNCES",TRUEV);
MAKE_CONST("LBS",LBS);
MAKE_CONST("DEG",TRUEV);
MAKE_CONST("DEGREES",TRUEV);
MAKE_CONST("NILVECT",NILVECT);
MAKE_CONST("NILROTN",NILROTN);
MAKE_CONST("NILTRANS",NILTRANS);
MAKE_CONST("STATION",STATION);
MAKE_CONST("XHAT",XHAT);
MAKE_CONST("YHAT",YHAT);
MAKE_CONST("ZHAT",ZHAT);
MAKE_CONST("YPARK",YPARK);
MAKE_CONST("BPARK",BPARK);
MAKE_CONST("NILDEPROACH",NILDEPROACH);
END;
REQUIRE IACNSTS INITIALIZATION [2];
INTEGER SIMPROC SIGNUM(REAL V);
START_CODE
LABEL XIT;
SKIPN 1,V;
JRST XIT;
MOVNI 2,1;
CAIL 1,0;
MOVEI 2,1;
MOVE 1,2;
XIT: END;
! new_sval,new_v3ect,new_trans,new_frame;
! these routines create new records of the indicated types
and store away their parameters into the appropriate subfields;
INTERNAL RPTR(SVAL) PROCEDURE NEW_SVAL(REAL VAL);
BEGIN
RPTR(SVAL) S;
S←NEW_RECORD(SVAL);
SVAL:VAL[S]←VAL;
RETURN(S);
END;
INTERNAL RPTR(V3ECT) PROCEDURE NEW_V3ECT(REAL X,Y,Z);
BEGIN
RPTR(V3ECT) V;
V←NEW_RECORD(V3ECT);
V3ECT:X[V]←X;
V3ECT:Y[V]←Y;
V3ECT:Z[V]←Z;
RETURN(V);
END;
INTERNAL RPTR(TRANS) PROCEDURE NEW_TRANS(RPTR(ROTN) R;RPTR(V3ECT) P);
BEGIN
RPTR(TRANS) T;
T←NEW_RECORD(TRANS);
TRANS:R[T]←R;
TRANS:P[T]←P;
RETURN(T);
END;
INTERNAL RPTR(FRAME) PROCEDURE NEW_FRAME(RPTR(TRANS) T;
RPTR(TINFO) TINF(NULL_RECORD));
BEGIN
RPTR(FRAME) F;
F←NEW_RECORD(FRAME);
FRAME:VAL[F]←T;
FRAME:TINFO[F]←TINF;
RETURN(F);
END;
! given a trans or frame, this routine returns a trans;
INTERNAL RPTR(TRANS) PROCEDURE TRANS_COERCE(RPTR(TRANS,FRAME) TF);
BEGIN
INTEGER RT;
RT←RECTYPE(TF);
IF RT=LOC(FRAME) THEN
RETURN(FRAME:VAL[TF])
ELSE IF RT=LOC(TRANS)∨RT=0 THEN
RETURN(TF)
ELSE
RETURN(CHKREC(TF,LOC(TRANS))); ! an error *** ;
END;
! v3dot, v3dist, v3angle, grfmul;
! Note: a number of these routines employ an optional parameter
to allow the user to specify the vector record to hold
the return value. If this parameter is omitted or null,
then a new record will be allocated to hold the result;
INTERNAL REAL SIMPLE PROCEDURE V3DOT(RPTR(V3ECT) V1,V2);
START_CODE
! returns x[v1]*x[v2]+y[v1]*y[v2]+z[v1]*v[v2] ;
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 1,1(5);
FMPR 1,1(6);
MOVE 2,2(5);
FMPR 2,2(6);
MOVE 3,3(5);
FMPR 3,3(6);
FADR 1,2;
FADR 1,3;
END;
INTERNAL REAL SIMPLE PROCEDURE V3MAGN(RPTR(V3ECT) V);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P ='17;
SKIPN 5,V;
PUSHJ P,$RERR;
MOVE 1,1(5);
FMPR 1,1;
MOVE 2,2(5);
FMPR 2,2;
MOVE 3,3(5);
FMPR 3,3;
FADR 1,2;
FADR 1,3;
PUSH P,1;
PUSHJ P,SQRT;
END;
INTERNAL REAL SIMPLE PROCEDURE V3DIST(RPTR(V3ECT) V1,V2);
START_CODE
! Returns distance between v1 & v2;
EXTERNAL INTEGER $RERR;
DEFINE P ='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 1,1(5);
FSBR 1,1(6);
FMPR 1,1;
MOVE 2,2(5);
FSBR 2,2(6);
FMPR 2,2;
MOVE 3,3(5);
FSBR 3,3(6);
FMPR 3,3;
FADR 1,2;
FADR 1,3;
PUSH P,1;
PUSHJ P,SQRT;
END;
INTERNAL REAL PROCEDURE V3ANGLE(RPTR(V3ECT) V1,V2);
RETURN( ACOS( V3DOT(V1,V2)/(V3MAGN(V1)*V3MAGN(V2)) )*DEG );
INTERNAL REAL PROCEDURE GRFMUL(RPTR(V3ECT) G;RPTR(ROTN) R;RPTR(V3ECT) F);
RETURN(V3DOT(G,RVMUL(R,F)));
! v3ect-valued procedures;
INTERNAL RPTR(V3ECT) PROCEDURE SVMUL(REAL SF;RPTR(V3ECT) V1);
BEGIN
! scales vector v1 by real sf & returns the result;
RPTR(V3ECT) V;
DEFINE XX "<>" = <V3ECT:X>;
DEFINE YY "<>" = <V3ECT:Y>;
DEFINE ZZ "<>" = <V3ECT:Z>;
IF SF=0 THEN RETURN(NILVECT);
IF SF=1.0 THEN RETURN(V1);
V←NEW_RECORD(V3ECT);
XX[V]←SF*XX[V1];
YY[V]←SF*YY[V1];
ZZ[V]←SF*ZZ[V1];
RETURN(V);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3CROSS(RPTR(V3ECT) A,B);
BEGIN
! computes cross(a,b);
RPTR(V3ECT) AXB;
DEFINE XX "<>" = <V3ECT:X>;
DEFINE YY "<>" = <V3ECT:Y>;
DEFINE ZZ "<>" = <V3ECT:Z>;
AXB←NEW_RECORD(V3ECT);
XX[AXB]←YY[A]*ZZ[B]-ZZ[A]*YY[B];
YY[AXB]←ZZ[A]*XX[B]-XX[A]*ZZ[B];
ZZ[AXB]←XX[A]*YY[B]-YY[A]*XX[B];
RETURN(AXB);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3ADD(RPTR(V3ECT) V1,V2);
BEGIN
RPTR(V3ECT) V3;
IF V1=NILVECT THEN RETURN(V2);
IF V2=NILVECT THEN RETURN(V1);
V3←NEW_RECORD(V3ECT);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 7,V3;
MOVE 1,1(5);
FADR 1,1(6);
MOVEM 1,1(7);
MOVE 1,2(5);
FADR 1,2(6);
MOVEM 1,2(7);
MOVE 1,3(5);
FADR 1,3(6);
MOVEM 1,3(7);
END;
RETURN(V3);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3SUB(RPTR(V3ECT) V1,V2);
BEGIN
RPTR(V3ECT) V3;
IF V2=NILVECT THEN RETURN(V1);
V3←NEW_RECORD(V3ECT);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 7,V3;
MOVE 1,1(5);
FSBR 1,1(6);
MOVEM 1,1(7);
MOVE 1,2(5);
FSBR 1,2(6);
MOVEM 1,2(7);
MOVE 1,3(5);
FSBR 1,3(6);
MOVEM 1,3(7);
END;
RETURN(V3);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3LINC(REAL W1;RPTR(V3ECT) V1;
REAL W2;RPTR(V3ECT) V2);
BEGIN
! returns w1*v1+w2*v2;
RPTR(V3ECT) V3;
V3←NEW_RECORD(V3ECT);
START_CODE
EXTERNAL INTEGER $RERR;
DEFINE P='17;
SKIPE 5,V1;
SKIPN 6,V2;
PUSHJ P,$RERR;
MOVE 7,V3;
! x coord;
MOVE 1,1(5);
FMPR 1,W1;
MOVE 2,1(6);
FMPR 2,W2;
FADR 1,2;
MOVEM 1,1(7);
! y coord;
MOVE 1,2(5);
FMPR 1,W1;
MOVE 2,2(6);
FMPR 2,W2;
FADR 1,2;
MOVEM 1,2(7);
! z coord;
MOVE 1,3(5);
FMPR 1,W1;
MOVE 2,3(6);
FMPR 2,W2;
FADR 1,2;
MOVEM 1,3(7);
END;
RETURN(V3);
END;
INTERNAL RPTR(V3ECT) PROCEDURE V3COPY(RPTR(V3ECT) V,NV(NULL_RECORD));
BEGIN
! returns a new vector with the same values as V;
IF NV=NULL_RECORD THEN
NV←NEW_RECORD(V3ECT);
V3ECT:X[NV]←V3ECT:X[V];
V3ECT:Y[NV]←V3ECT:Y[V];
V3ECT:Z[NV]←V3ECT:Z[V];
RETURN(NV);
END;
INTERNAL RPTR(V3ECT) PROCEDURE RVMUL(RPTR(ROTN) R;RPTR(V3ECT) V);
BEGIN
! produces a new vector with value R*V;
REAL X,Y,Z;
INTEGER I,II;
IF R=NILROTN ∨ V=NILVECT THEN RETURN(V);
II←MEM[LOC(V)]; X←Y←Z←0;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
X←X+MEM[II+I,REAL]*ROTN:RMX[R][I,1];
Y←Y+MEM[II+I,REAL]*ROTN:RMX[R][I,2];
Z←Z+MEM[II+I,REAL]*ROTN:RMX[R][I,3];
END;
RETURN(NEW_V3ECT(X,Y,Z));
END;
INTERNAL RPTR(V3ECT) PROCEDURE RIVMUL(RPTR(ROTN) R;RPTR(V3ECT) V);
BEGIN
! produces a new vector with value INV(R)*V;
REAL X,Y,Z;
INTEGER I,II;
IF R=NILROTN ∨ V=NILVECT THEN RETURN(V);
II←MEM[LOC(V)]; X←Y←Z←0;
FOR I←1 STEP 1 UNTIL 3 DO
BEGIN
X←X+MEM[II+I,REAL]*ROTN:RMX[R][1,I];
Y←Y+MEM[II+I,REAL]*ROTN:RMX[R][2,I];
Z←Z+MEM[II+I,REAL]*ROTN:RMX[R][3,I];
END;
RETURN(NEW_V3ECT(X,Y,Z));
END;
INTERNAL RPTR(V3ECT) PROCEDURE TVMUL(RPTR(TRANS) T;RPTR(V3ECT) V);
RETURN(V3ADD(TRANS:P[T],RVMUL(TRANS:R[T],V)));
INTERNAL RPTR(V3ECT) PROCEDURE TIVMUL(RPTR(TRANS) T;RPTR(V3ECT) V);
RETURN(RIVMUL(TRANS:R[T],V3SUB(V,TRANS:P[T])));
INTERNAL RPTR(V3ECT) PROCEDURE UVECT(RPTR(V3ECT) V;REAL SLOPOK(0.0));
BEGIN
! this procedure transforms V into a unit vector & returns VHAT
IF abs(v.v-1.0)≤slopok then assumes that vector is already unit.
;
REAL M;
M←V3DOT(V,V);
IF ABS(M-1.0)≤SLOPOK THEN
RETURN(V)
ELSE IF M=0 THEN
BEGIN
USERERR(1,1,"UVECT: 0 VECTOR");
RETURN(V);
END
ELSE
RETURN(SVMUL(1/SQRT(M),V));
END;
! rotn valued procedures;
RPTR(ROTN) PROCEDURE AXW_R(RPTR(V3ECT) AX;
REAL W;RPTR(ROTN) R(NULL_RECORD));
! **** WARNING: Do NOT supply a rotation R
to this unless αL & βL are correct ****;
BEGIN
! produces a ROTN, given axis vector AX & magnitude W;
SAFE REAL ARRAY RMX[1:3,1:3];
REAL CX,CY,CZ,SW,CW;
AX←UVECT(AX);
CX←V3ECT:X[AX];
CY←V3ECT:Y[AX];
CZ←V3ECT:Z[AX];
IF R=NULL_RECORD THEN
BEGIN
R←NEW_RECORD(ROTN);
ROTN:αL[R]←ACOS(CZ)*DEG;
ROTN:βL[R]←ATAN2(CY,CX)*DEG;
END;
ROTN:MAGN[R]←W;
ROTN:AXIS[R]←AX;
SW←SIND(W);CW←COSD(W);
RMX[1,1]←CW+(1-CW)*CX↑2;
RMX[2,1]←(1-CW)*CX*CY-CZ*SW;
RMX[3,1]←(1-CW)*CX*CZ+CY*SW;
RMX[1,2]←(1-CW)*CX*CY+CZ*SW;
RMX[2,2]←CW+(1-CW)*CY↑2;
RMX[3,2]←(1-CW)*CY*CZ-CX*SW;
RMX[1,3]←(1-CW)*CX*CZ-CY*SW;
RMX[2,3]←(1-CW)*CY*CZ+CX*SW;
RMX[3,3]←CW+(1-CW)*CZ↑2;
MEM[LOC(ROTN:RMX[R])]←MEM[LOC(RMX)];
MEM[LOC(RMX)]←0;
RETURN(R);
END;
INTERNAL RPTR(ROTN) PROCEDURE AXW_ROTN(RPTR(V3ECT) AX;REAL W);
RETURN(AXW_R(AX,W));
INTERNAL RPTR(ROTN) PROCEDURE RINVRT(RPTR(ROTN) R);
BEGIN
! returns inverse rotation to R;
RPTR(ROTN) RINV;
IF R=NILROTN THEN RETURN(NILROTN);
BEGIN
SAFE REAL ARRAY RMX[1:3,1:3];
RINV←NEW_RECORD(ROTN);
ARRTRAN(RMX,ROTN:RMX[R]);
ROTN:AXIS[RINV]←ROTN:AXIS[R];
ROTN:αL[RINV]←ROTN:αL[R];
ROTN:βL[RINV]←ROTN:βL[R];
MEM[LOC(ROTN:RMX[RINV])]↔MEM[LOC(RMX)];
END;
ROTN:RMX[RINV][1,2]↔ROTN:RMX[RINV][2,1];
ROTN:RMX[RINV][1,3]↔ROTN:RMX[RINV][3,1];
ROTN:RMX[RINV][2,3]↔ROTN:RMX[RINV][3,2];
ROTN:MAGN[RINV]←-ROTN:MAGN[R];
RETURN(RINV);
END;
INTERNAL RPTR(ROTN) PROCEDURE αβW_ROTN(REAL αL,βL,W);
BEGIN
! builds a rotation of magn W about an axis specified by
angles αL & βL;
RPTR(ROTN) R;
R←NEW_RECORD(ROTN);
ROTN:αL[R]←αL;ROTN:βL[R]←βL;
RETURN(
AXW_R( NEW_V3ECT(SIND(αL)*COSD(βL),SIND(αL)*SIND(βL),COSD(αL)),
W,
R));
END;
INTERNAL RPTR(ROTN) PROCEDURE RRMUL(RPTR(ROTN) R1,R2);
BEGIN
! produces a new rotation equiv to R1*R2;
SAFE OWN REAL ARRAY RMX[1:3,1:3];
INTEGER I,J,K,L;
DEFINE XX(I) "<>" = <RMX[I,1]>;
DEFINE YY(I) "<>" = <RMX[I,2]>;
DEFINE ZZ(I) "<>" = <RMX[I,3]>;
! ugh! blech! column order sure is confusing when you
have been using row order for the past eight years! ;
ARRCLR(RMX);
FOR I←1 STEP 1 UNTIL 3 DO
FOR J←1 STEP 1 UNTIL 2 DO
FOR K←1 STEP 1 UNTIL 3 DO
RMX[J,I]←RMX[J,I]+ROTN:RMX[R1][K,I]*ROTN:RMX[R2][J,K];
XX(3)←YY(1)*ZZ(2)-ZZ(1)*YY(2);
YY(3)←ZZ(1)*XX(2)-XX(1)*ZZ(2);
ZZ(3)←XX(1)*YY(2)-YY(1)*XX(2);
RETURN(RMX_ROTN(RMX));
END;
INTERNAL RPTR(ROTN) PROCEDURE VTOV_R(RPTR(V3ECT) V1,V2;REAL SLOPOK(0.0));
BEGIN
! returns a rotation that will make V1 lie parallel to
V2. If the cosine of the angle between V1 & V2 is
less than SLOPOK, then NILROTN will be returned.
;
RPTR(V3ECT) V3;
REAL C,W0;
V1←UVECT(V1);V2←UVECT(V2);
C←V3DOT(V1,V2);
IF ABS(1.0-C)≤SLOPOK THEN
RETURN(NILROTN); ! treat these vectors as parallel;
IF ABS(1.0+C)≤SLOPOK THEN
BEGIN ! they are 180 degrees apart;
IF ABS(V3ECT:X[V1])<.6 THEN V3←XHAT
ELSE IF ABS(V3ECT:Y[V1])<.6 THEN V3←YHAT
ELSE IF ABS(V3ECT:Z[V1])<.6 THEN V3←ZHAT;
V3←V3CROSS(V1,V3);
W0←180;
END
ELSE
BEGIN
V3←V3CROSS(V1,V2);
W0←ATAN2(V3MAGN(V3),C)*DEG;
END;
RETURN(AXW_ROTN(V3,W0));
END;
INTERNAL RPTR(ROTN) PROCEDURE VVROT(RPTR(V3ECT) X,Z);
BEGIN
! returns a rotation that will put ZHAT parallel to
Z & which will put XHAT into the XZ plane.;
RPTR(ROTN) R;
R←VTOV_R(ZHAT,Z);
X←V3LINC(1.0,X,-V3DOT(X,Z)/V3DOT(Z,Z),Z);
RETURN(RRMUL(VTOV_R(RVMUL(R,XHAT),X),R));
END;
! rotn extraction procedures;
SIMPLE REAL PROCEDURE SSQRT(REAL V);
RETURN(IF -0.000001<V<0 THEN 0.0 ELSE SQRT(V));
SIMPLE REAL PROCEDURE AACOS(REAL C);
RETURN(IF -1.000001<C<-1.0 THEN π
ELSE IF 1.0000<C<1.000001 THEN 0.0
ELSE ACOS(C));
INTERNAL RPTR(V3ECT) PROCEDURE AXIS(RPTR(ROTN) R);
BEGIN
! extracts the axis of rotation from a ROTN;
! Written by ARG 6-76;
REAL CX,CY,CZ,A,B,C,CW;
A←ROTN:RMX[R][1,1];
B←ROTN:RMX[R][2,2];
C←ROTN:RMX[R][3,3];
CW←(A+B+C-1)/2;
IF CW >0.9999 THEN
RETURN(ZHAT) ! vector for NILROT;
ELSE
BEGIN
REAL D;
D←3-A-B-C;
CZ←SSQRT((-A-B+C+1)/D);
CY ← IF CZ<0.001 THEN SSQRT((-A+B-C+1)/D)
ELSE (ROTN:RMX[R][2,3]
- ROTN:RMX[R][2,1]*ROTN:RMX[R][1,3]/(A-1))*CZ
/ (1-B+ROTN:RMX[R][2,1]*ROTN:RMX[R][1,2]/(A-1));
CX ← IF CZ+ABS(CY)<0.001 THEN SSQRT((A-B-C+1)/D)
ELSE -(ROTN:RMX[R][1,2]*CY + ROTN:RMX[R][1,3]*CZ) / (A-1);
RETURN(NEW_V3ECT(CX,CY,CZ));
END
END;
INTERNAL RPTR(SVAL) PROCEDURE RMAGN(RPTR(ROTN) R);
BEGIN
! finds the angle of rotation for a ROTN;
! Written by ARG 6-76;
REAL CX,CY,CZ,A,B,C,CW;
RPTR(SVAL) S;
S←NEW_RECORD(SVAL);
A←ROTN:RMX[R][1,1];
B←ROTN:RMX[R][2,2];
C←ROTN:RMX[R][3,3];
CW←(A+B+C-1)/2;
IF CW >0.9999 THEN
SVAL:VAL[S]←0
ELSE
BEGIN
REAL D;
D←3-A-B-C;
CZ←SSQRT((-A-B+C+1)/D);
CY ← IF CZ<0.001 THEN SSQRT((-A+B-C+1)/D)
ELSE (ROTN:RMX[R][2,3]
- ROTN:RMX[R][2,1]*ROTN:RMX[R][1,3]/(A-1))*CZ
/ (1-B+ROTN:RMX[R][2,1]*ROTN:RMX[R][1,2]/(A-1));
CX ← IF CZ+ABS(CY)<0.001 THEN SSQRT((A-B-C+1)/D)
ELSE -(ROTN:RMX[R][1,2]*CY + ROTN:RMX[R][1,3]*CZ) / (A-1);
SVAL:VAL[S]←AACOS(CW)*DEG;
IF ABS(CZ) ≥ 0.577 THEN
BEGIN
IF (ROTN:RMX[R][2,1]-ROTN:RMX[R][1,2])/CZ > 0 THEN
SVAL:VAL[S]←-SVAL:VAL[S];
END
ELSE IF ABS(CY)≥0.577 THEN
BEGIN
IF (ROTN:RMX[R][1,3]-ROTN:RMX[R][3,1])/CY > 0 THEN
SVAL:VAL[S]←-SVAL:VAL[S];
END
ELSE IF ABS(CX)≥0.577 THEN
BEGIN
IF (ROTN:RMX[R][3,2]-ROTN:RMX[R][2,3])/CX > 0 THEN
SVAL:VAL[S]←-SVAL:VAL[S];
END
ELSE
USERERR(1,1,"RMAGN: STRANGENESS!");
END;
RETURN(S);
END;
INTERNAL RPTR(ROTN) PROCEDURE RMX_ROTN(REAL ARRAY RMXX);
BEGIN
! builds a rotation, given a rotation matrix RMXX.
Does not affect RMXX;
! Modified to work properly by ARG 6-76;
SAFE REAL ARRAY RMX[1:3,1:3];
REAL CX,CY,CZ,A,B,C,CW;
RPTR(ROTN) R;
ARRTRAN(RMX,RMXX);
A←RMX[1,1];B←RMX[2,2];C←RMX[3,3];
CW←(A+B+C-1)/2;
IF CW >0.9999 THEN
RETURN(NILROTN)
ELSE
BEGIN
REAL D;
R←NEW_RECORD(ROTN);
D←3-A-B-C;
CZ←SSQRT((-A-B+C+1)/D);
CY ← IF CZ<0.001 THEN SSQRT((-A+B-C+1)/D)
ELSE (RMX[2,3] - RMX[2,1]*RMX[1,3]/(A-1))*CZ
/ (1-B+RMX[2,1]*RMX[1,2]/(A-1));
CX ← IF CZ+ABS(CY)<0.001 THEN SSQRT((A-B-C+1)/D)
ELSE -(RMX[1,2]*CY + RMX[1,3]*CZ) / (A-1);
ROTN:AXIS[R]←NEW_V3ECT(CX,CY,CZ);
ROTN:MAGN[R]←AACOS(CW)*DEG;
IF ABS(CZ) ≥ 0.577 THEN
BEGIN
IF (RMX[2,1]-RMX[1,2])/CZ > 0 THEN
ROTN:MAGN[R]←-ROTN:MAGN[R];
END
ELSE IF ABS(CY)≥0.577 THEN
BEGIN
IF (RMX[1,3]-RMX[3,1])/CY > 0 THEN
ROTN:MAGN[R]←-ROTN:MAGN[R];
END
ELSE IF ABS(CX)≥0.577 THEN
BEGIN
IF (RMX[3,2]-RMX[2,3])/CX > 0 THEN
ROTN:MAGN[R]←-ROTN:MAGN[R];
END
ELSE
USERERR(1,1,"RMX_ROTN: STRANGENESS!");
ROTN:αL[R]←AACOS(CZ)*DEG;
ROTN:βL[R]←ATAN2(CY,CX)*DEG;
END;
MEM[LOC(ROTN:RMX[R])]←MEM[LOC(RMX)];
MEM[LOC(RMX)]←0;
RETURN(R);
END;
INTERNAL RPTR(ROTN) PROCEDURE ORIENT(RPTR(TRANS) T);
BEGIN
RPTR(ROTN) R;
R←TRANS:R[T];
RETURN(RMX_ROTN(ROTN:RMX[R]));
END;
INTERNAL RPTR(V3ECT) PROCEDURE POS(RPTR(TRANS) T);
BEGIN
RPTR(V3ECT) V,W;
W←NEW_RECORD(V3ECT);
V←TRANS:P[T];
V3ECT:X[W]←V3ECT:X[V];
V3ECT:Y[W]←V3ECT:Y[V];
V3ECT:Z[W]←V3ECT:Z[V];
RETURN(W);
END;
! trans & frame valued procedures;
INTERNAL RPTR(TRANS) PROCEDURE TTMUL(RPTR(TRANS) T1,T2);
BEGIN
! returns trans T1*T2;
RPTR(TRANS) T;
IF T1=NILTRANS THEN RETURN(T2);
IF T2=NILTRANS THEN RETURN(T1);
T←NEW_RECORD(TRANS);
TRANS:R[T]←RRMUL(TRANS:R[T1],TRANS:R[T2]);
TRANS:P[T]←V3ADD(TRANS:P[T1],RVMUL(TRANS:R[T1],TRANS:P[T2]));
RETURN(T);
END;
INTERNAL RPTR(TRANS) PROCEDURE TINVRT(RPTR(TRANS) T);
BEGIN
! returns inverse trans to T;
RPTR(TRANS) TINV;
IF T=NILTRANS THEN RETURN(NILTRANS);
TINV←NEW_RECORD(TRANS);
TRANS:R[TINV]←RINVRT(TRANS:R[T]);
TRANS:P[TINV]←SVMUL(-1.0,RVMUL(TRANS:R[TINV],TRANS:P[T]));
RETURN(TINV);
END;
INTERNAL RPTR(TRANS) PROCEDURE VVVTRANS(RPTR(V3ECT) P,X,Z);
RETURN(NEW_TRANS(VVROT(V3SUB(X,P),V3SUB(Z,P)),P));
! ie a trans that moves the origin to P, tilts the
z-axis to point along z-p & puts xhat into the
(z-p)(x-p) plane;
INTERNAL RPTR(TRANS) PROCEDURE CONSTR(RPTR(V3ECT) P,X,XY);
RETURN(NEW_TRANS(VVROT(V3SUB(X,P),V3CROSS(V3SUB(X,P),V3SUB(XY,P))),P));
! ie a trans that moves the origin to P, tilts the
z-axis to point along (x-p) ⊗ (xy-p) & puts xhat
into the (z)(x-p) plane;
! v3cmp, rotcmp, & transcmp;
! These do lexical comparisons of vectors & transform matrices.
-1 ... arg1 < arg2
0 ... arg1 = arg2
+1 ... arg1 > arg2
;
INTERNAL INTEGER PROCEDURE V3CMP(RPTR(V3ECT) V1,V2);
START_CODE
LABEL XIT,L;
MOVEI 1,0;
MOVE 2,V1;
MOVE 3,V2;
MOVE 4,[(-3 LSH 18) + 1];
HRLI 2,4;
HRLI 3,4;
L: MOVE 5,@2;
CAMGE 5,@3;
SOJA 1,XIT;
CAME 5,@3;
AOJA 1,XIT;
AOBJN 4,L;
XIT: END;
INTERNAL INTEGER PROCEDURE ROTCMP(RPTR(ROTN) R1,R2);
BEGIN
INTEGER I;
I←V3CMP(ROTN:AXIS[R1],ROTN:AXIS[R2]);
RETURN(IF I THEN I ELSE SIGNUM(ROTN:MAGN[R1]-ROTN:MAGN[R2]));
END;
INTERNAL INTEGER PROCEDURE TRANSCMP(RPTR(TRANS) T1,T2);
BEGIN
INTEGER I;
I←V3CMP(TRANS:P[T1],TRANS:P[T2]);
RETURN(IF I THEN I ELSE ROTCMP(TRANS:R[T1],TRANS:R[T2]));
END;
END "ARITH";